/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file roombaTranThread.h
	\brief header file for roombaTranThread

                    \author Jack Elston
             $Date: 2006/10/25 13:34:45 $
*/

#ifndef __ROOMBATRANTHREAD_H
#define __ROOMBATRANTHREAD_H

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "String.h"
#include "command.h"
#include "queue.h"
#include "type_defs.h"
#include "threadedPipe.h"
#include "shmStruct.h"

#define SCI_START              128
#define SCI_BAUD               129
#define SCI_CONTROL            130
#define SCI_SAFE               131
#define SCI_FULL               132
#define SCI_POWER              133
#define SCI_SPOT               134
#define SCI_CLEAN              135
#define SCI_MAX                136
#define SCI_DRIVE              137
#define SCI_MOTORS             138
#define SCI_LEDS               139
#define SCI_SONG               140
#define SCI_PLAY               141
#define SCI_SENSORS            142
#define SCI_FORCE_SEEKING_DOCK 143

#define ROOMBA_NOT_CHARGING      0
#define ROOMBA_CHARGING_RECOVERY 1
#define ROOMBA_CHARGING          2
#define ROOMBA_TRICKLE_CHARGING  3
#define ROOMBA_WAITING           4
#define ROOMBA_CHARGING_ERROR    5

typedef enum {
	UNKNOWN,
	NOT_COMMANDABLE,
	WAITING_REPLY,
	READY_FOR_COMMAND,
	CLIFF_SENSOR,
} RoombaState_t;

#define NOT_REQUESTED 5

#define STATUS_REQUEST_PERIOD 0.1
#define STATE_CHANGE_PERIOD   1.0
#define MAX_SCI_PACKET_SIZE   26
#define MIN_SCI_PACKET_SIZE   6

class RoombaTranThread : public ThreadedPipe
{
private:
  RoombaState_t roomba_state;
	SCISensorPacket_t sensor_packet;
	bool received_info;
	uint8_t info_requested;
	uint8_t buf_ptr;
	uint8_t buf[MAX_SCI_PACKET_SIZE];
	int num_retries;

	TimeStamp last_status_request;
	TimeStamp last_state_change;

  void initialize();
	void requestSensorInfo(uint8_t type);
	void drive(int velocity, int radius);

  void parseRoombaData(int n);
  void parseInternalData(const Command &cmd);

	timespec t;

public:
	RoombaTranThread(String name);
	~RoombaTranThread() {}
					 
	void update();
};

#endif
